//////////////////////////////////////////////////////////////////////
// Matrix44.cpp - 4 x 4 transformation matrices.
//
// Copyright David K. McAllister, Aug. 1998.
// Originally written by Steven G. Parker, Feb. 1994.
// I don't think the matrix invert is by Steve. It had a bug.

#include "Math/Matrix44.h"
#include "Math/Vector.h"

//////////////////////////////////////////////////////////
// Private member functions.

void Matrix44::build_identity(double m[4][4]) const
{
    m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
    m[0][1] = m[0][2] = m[0][3] = 0.0;
    m[1][0] = m[1][2] = m[1][3] = 0.0;
    m[2][0] = m[2][1] = m[2][3] = 0.0;
    m[3][0] = m[3][1] = m[3][2] = 0.0;
}

void Matrix44::build_scale(double m[4][4], const Vector& v) const
{
    build_identity(m);
    m[0][0]=v.x;
    m[1][1]=v.y;
    m[2][2]=v.z;
}

void Matrix44::build_translate(double m[4][4], const Vector& v) const
{
    build_identity(m);
    m[0][3]=v.x;
    m[1][3]=v.y;
    m[2][3]=v.z;
}

void Matrix44::build_rotate(double m[4][4], double angle, const Vector& axis) const
{
    // NOTE: Element 0,1 is wrong in Foley and Van Dam, Pg 227!
    double sintheta=sin(angle);
    double costheta=cos(angle);
    double ux=axis.x;
    double uy=axis.y;
    double uz=axis.z;
    m[0][0]=ux*ux+costheta*(1-ux*ux);
    m[0][1]=ux*uy*(1-costheta)-uz*sintheta;
    m[0][2]=uz*ux*(1-costheta)+uy*sintheta;
    m[0][3]=0;

    m[1][0]=ux*uy*(1-costheta)+uz*sintheta;
    m[1][1]=uy*uy+costheta*(1-uy*uy);
    m[1][2]=uy*uz*(1-costheta)-ux*sintheta;
    m[1][3]=0;

    m[2][0]=uz*ux*(1-costheta)-uy*sintheta;
    m[2][1]=uy*uz*(1-costheta)+ux*sintheta;
    m[2][2]=uz*uz+costheta*(1-uz*uz);
    m[2][3]=0;

    m[3][0]=0;
    m[3][1]=0;
    m[3][2]=0;
    m[3][3]=1;
}

// Returns true if it worked, false if non-invertible.
bool Matrix44::build_inverse(double m[4][4]) const
{
    double p[4][4];
    build_identity(p);

    // Make it upper triangular using Gauss-Jordan with partial pivoting.
    int i;
    for(i=0; i<4; i++)
    {
        // Find largest row.
        double max=Abs(m[i][i]);
        int row=i;
        int j;
        for(j=i+1; j<4; j++)
        {
            if(Abs(m[j][i]) > max)
            {
                max=Abs(m[j][i]);
                row=j;
            }
        }

        // Pivot around largest row.
        if(max <= singularity_thresh)
            return false;

        if(row!=i)
        {
            switch_rows(m, i, row);
            switch_rows(p, i, row);
        }

        // Subtract scaled rows to eliminate column i.
        ASSERT_D(m[i][i]!=0);
        double denom = 1./m[i][i];
        for(j=i+1; j<4; j++)
        {
            double factor = m[j][i] * denom;
            sub_rows(m, j, i, factor);
            sub_rows(p, j, i, factor);
        }
    }

    // Diagonalize m using Jordan.
    for(i=1; i<4; i++)
    {
        ASSERT_D(m[i][i]!=0);
        double denom = 1./m[i][i];
        for(int j=0; j<i; j++)
        {
            double factor = m[j][i] * denom;
            sub_rows(m, j, i, factor);
            sub_rows(p, j, i, factor);
        }
    }

    // Normalize m to the identity and copy p over m.
    for(i=0; i<4; i++)
    {
        ASSERT_D(m[i][i]!=0);
        double factor = 1./m[i][i];
        for(int j=0; j<4; j++)
        {
            // As if we were doing m[i][j] *= factor
            p[i][j] *= factor;
            m[i][j] = p[i][j];
        }
    }

    return true;
}

void Matrix44::build_transpose(double m[4][4]) const
{
    double t;
    t = m[0][1]; m[0][1] = m[1][0]; m[1][0] = t;
    t = m[0][2]; m[0][2] = m[2][0]; m[2][0] = t;
    t = m[0][3]; m[0][3] = m[3][0]; m[3][0] = t;
    t = m[1][2]; m[1][2] = m[2][1]; m[2][1] = t;
    t = m[1][3]; m[1][3] = m[3][1]; m[3][1] = t;
    t = m[2][3]; m[2][3] = m[3][2]; m[3][2] = t;
}

void Matrix44::post_mulmat(double to[4][4], const double from[4][4])
{
    double newmat[4][4];
    for(int i=0; i<4; i++)
    {
        for(int j=0; j<4; j++)
        {
            newmat[i][j] = 0.0;
            for(int k=0; k<4; k++)
            {
                newmat[i][j] += to[i][k] * from[k][j];
            }
        }
    }

    copy_mat(to, newmat);
}

void Matrix44::pre_mulmat(double to[4][4], const double from[4][4])
{
    double newmat[4][4];
    for(int i=0; i<4; i++)
    {
        for(int j=0; j<4; j++)
        {
            newmat[i][j]=0.0;
            for(int k=0; k<4; k++)
            {
                newmat[i][j] += from[i][k] * to[k][j];
            }
        }
    }

    copy_mat(to, newmat);
}

//////////////////////////////////////////////////////////
// Public member functions.

std::string Matrix44::string() const
{
    const int PRDIG = 8;
    char xx[40];
    std::string st;
    for(int i=0; i<4; i++) {
        st += std::string("[");
        st += gcvt(mat[i][0], PRDIG, xx) + std::string(", ");
        st += gcvt(mat[i][1], PRDIG, xx) + std::string(", ");
        st += gcvt(mat[i][2], PRDIG, xx) + std::string(", ");
        st += gcvt(mat[i][3], PRDIG, xx) + std::string("]\n");
    }

    return st;
}

std::string Matrix44::string_inv() const
{
    const int PRDIG = 8;
    char xx[40];
    std::string st;
    for(int i=0; i<4; i++)
    {
        st += std::string("[");
        for(int j=0; j<4; j++)
            st += gcvt(imat[i][j], PRDIG, xx) + std::string(", ");
        st += std::string("]\n");
    }

    return st;
}

double Matrix44::Determinant()
{
    return mat[0][0] * det3(0) - mat[0][1] * det3(1) +
        mat[0][2] * det3(2) - mat[0][3] * det3(3);
}

void Matrix44::PostTrans(const Matrix44& T)
{
    if(T.is_identity)
        return;
    if(is_identity)
    {
        // Copying is faster than matrix multiply.
        copy_mat(mat, T.mat);
        copy_mat(imat, T.imat);
        is_identity = false;
        inverse_valid = T.inverse_valid;
        return;
    }

    post_mulmat(mat, T.mat);
    pre_mulmat(imat, T.imat);
    inverse_valid = inverse_valid && T.inverse_valid;
}

void Matrix44::PreTrans(const Matrix44& T)
{
    if(T.is_identity)
        return;
    if(is_identity)
    {
        // Copying is faster than matrix multiply.
        copy_mat(mat, T.mat);
        copy_mat(imat, T.imat);
        is_identity = false;
        inverse_valid = T.inverse_valid;
        return;
    }

    pre_mulmat(mat, T.mat);
    post_mulmat(imat, T.imat);
    inverse_valid = inverse_valid && T.inverse_valid;
}

// Load these column vectors. Rest of matrix is identity.
// Assumes x,y,z form an orthonormal basis so that the inverse is the transpose.
void Matrix44::LoadFrame(const Vector& x, const Vector& y, const Vector& z)
{
    mat[3][3] = imat[3][3] = 1.0;
    mat[0][3] = mat[1][3] = mat[2][3] = 0.0;
    imat[0][3] = imat[1][3] = imat[2][3] = 0.0;

    mat[3][0] = mat[3][1] = mat[3][2] = 0.0;
    imat[3][0] = imat[3][1] = imat[3][2] = 0.0;

    mat[0][0] = x.x;
    mat[1][0] = x.y;
    mat[2][0] = x.z;

    mat[0][1] = y.x;
    mat[1][1] = y.y;
    mat[2][1] = y.z;

    mat[0][2] = z.x;
    mat[1][2] = z.y;
    mat[2][2] = z.z;

    imat[0][0] = x.x;
    imat[0][1] = x.y;
    imat[0][2] = x.z;

    imat[1][0] = y.x;
    imat[1][1] = y.y;
    imat[1][2] = y.z;

    imat[2][0] = z.x;
    imat[2][1] = z.y;
    imat[2][2] = z.z;

    inverse_valid = true;
    is_identity = false;
}

// Loads a rotation frame and an offset.
void Matrix44::LoadFrame(const Vector& x, const Vector& y,
                         const Vector& z, const Vector& t)
{
    LoadFrame(x, y, z);
    Translate(t);
}

void Matrix44::GetFrame(Vector &c0, Vector &c1, Vector &c2)
{
    c0.x = mat[0][0]; c1.x = mat[0][1]; c2.x = mat[0][2];
    c0.y = mat[1][0]; c1.y = mat[1][1]; c2.y = mat[1][2];
    c0.z = mat[2][0]; c1.z = mat[2][1]; c2.z = mat[2][2];
}

void Matrix44::GetFrame(Vector &c0, Vector &c1, Vector &c2, Vector &c3)
{
    c0.x = mat[0][0]; c1.x = mat[0][1]; c2.x = mat[0][2]; c3.x = mat[0][3];
    c0.y = mat[1][0]; c1.y = mat[1][1]; c2.y = mat[1][2]; c3.y = mat[1][3];
    c0.z = mat[2][0]; c1.z = mat[2][1]; c2.z = mat[2][2]; c3.z = mat[2][3];
}

void Matrix44::ChangeBasis(const Matrix44& T)
{
    // If T.imat is invalid it will not only make
    // imat invalid, but mat will be invalid.
    if(!inverse_valid)
        compute_inverse();

    // XXX Not yet optimized for the identity.
    pre_mulmat(mat, T.imat);
    post_mulmat(mat, T.mat);

    // XXX I need to check this.
    pre_mulmat(imat, T.imat);
    post_mulmat(imat, T.mat);
    is_identity = false;
}

void Matrix44::Scale(const Vector& v)
{
    double m[4][4];
    build_scale(m, v);
    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    m[0][0] = 1. / m[0][0];
    m[1][1] = 1. / m[1][1];
    m[2][2] = 1. / m[2][2];
    if(is_identity)
        copy_mat(imat, m);
    else
        pre_mulmat(imat, m);
    is_identity = false;
}

void Matrix44::Rotate(double angle, const Vector& axis)
{
    if(angle == 0)
        return;

    double m[4][4];
    build_rotate(m, angle, axis);
    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    build_transpose(m);
    if(is_identity)
        copy_mat(imat, m);
    else
        pre_mulmat(imat, m);
    is_identity = false;
}

void Matrix44::Translate(const Vector& v)
{
    double m[4][4];
    build_translate(m, v);
    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    m[0][3] = -m[0][3];
    m[1][3] = -m[1][3];
    m[2][3] = -m[2][3];

    if(is_identity)
        copy_mat(imat, m);
    else
        pre_mulmat(imat, m);
    is_identity = false;
}

bool Matrix44::Invert()
{
    if(inverse_valid)
    {
        if(is_identity) return true;

        // Just swap it with its inverse.
        double temp[4][4];
        copy_mat(temp, mat);
        copy_mat(mat, imat);
        copy_mat(imat, temp);
        return true;
    } else {
        // Copy mat to imat, then invert old mat.
        copy_mat(imat, mat);
        inverse_valid = true;
        if(is_identity) return true;

        return inverse_valid = build_inverse(mat);
    }
}

void Matrix44::Transpose()
{
    if(is_identity) return;

    build_transpose(mat);

    // XXX I don't know what to do to the inverse, so toss it.
    inverse_valid = false;
}

void Matrix44::Frustum(double l, double r, double b, double t, double n, double f)
{
    ASSERT_R(n>0 && f>0);
    double m[4][4];

    m[0][0] = (n+n)/(r-l);
    m[0][1] = 0;
    m[0][2] = (r+l)/(r-l);
    m[0][3] = 0;

    m[1][0] = 0;
    m[1][1] = (n+n)/(t-b);
    m[1][2] = (t+b)/(t-b);
    m[1][3] = 0;

    m[2][0] = 0;
    m[2][1] = 0;
    m[2][2] = -(f+n)/(f-n);   // On the OpenGL man page, these aren't negative.
    m[2][3] = -2.0*f*n/(f-n);

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = -1;
    m[3][3] = 0;

    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    m[0][0] = (r-l)/(n+n);
    m[0][1] = 0;
    m[0][2] = 0;
    m[0][3] = (r+l)/(n+n);

    m[1][0] = 0;
    m[1][1] = (t-b)/(n+n);
    m[1][2] = 0;
    m[1][3] = (t+b)/(n+n);

    m[2][0] = 0;
    m[2][1] = 0;
    m[2][2] = 0;
    m[2][3] = -1;

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = -(f-n)/(2.0*f*n);
    m[3][3] = (f+n)/(2.0*f*n);

    if(is_identity) {
        copy_mat(imat, m);
        inverse_valid = true;
    } else {
        if(inverse_valid) pre_mulmat(imat, m);
    }

    is_identity = false;
}

// n and f are negative if view plane is behind eye.
void Matrix44::Ortho(double l, double r, double b, double t, double n, double f)
{
    double m[4][4];

    m[0][0] = 2.0/(r-l);
    m[0][1] = 0;
    m[0][2] = 0;
    m[0][3] = (r+l)/(r-l);

    m[1][0] = 0;
    m[1][1] = 2.0/(t-b);
    m[1][2] = 0;
    m[1][3] = (t+b)/(t-b);

    m[2][0] = 0;
    m[2][1] = 0;
    m[2][2] = -2.0/(f-n);   // The signs of these were a pain.
    m[2][3] = -(f+n)/(f-n); // These signs make it work for OpenGL.

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;

    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    is_identity = false;
    // XXX I don't know what to do to the inverse, so toss it.
    inverse_valid = false;
}

void Matrix44::Perspective(double fovy, double aspect, double znear, double zfar)
{
    ASSERT_D(znear>0 && zfar>0);
    double top = znear * tan(fovy * 0.5);
    double bottom = -top;
    double left = bottom * aspect;
    double right = top * aspect;
    Frustum(left, right, bottom, top, znear, zfar);
}

void Matrix44::LookAt(const Vector& eye, const Vector& lookat, const Vector& up)
{
    Vector f(lookat - eye);
    f.normalize();

    Vector upn(up);
    upn.normalize();

    Vector s(Cross(f, upn));
    s.normalize();

    Vector u(Cross(s, f));
    // u.normalize(); // This normalize shouldn't be necessary.

    double m[4][4];

    m[0][0] = s.x;
    m[0][1] = s.y;
    m[0][2] = s.z;
    m[0][3] = 0;

    m[1][0] = u.x;
    m[1][1] = u.y;
    m[1][2] = u.z;
    m[1][3] = 0;

    m[2][0] = -f.x;
    m[2][1] = -f.y;
    m[2][2] = -f.z;
    m[2][3] = 0;

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;

    if(is_identity)
        copy_mat(mat, m);
    else
        post_mulmat(mat, m);

    build_transpose(m);

    if(is_identity)
        copy_mat(imat, m);
    else
        pre_mulmat(imat, m);

    is_identity = false;

    Translate(-eye);
}

Vector Matrix44::Project(const Vector& p) const
{
    // XXX Should I put an optimization here for is_identity?

    double w1 = mat[3][0]*p.x + mat[3][1]*p.y + mat[3][2]*p.z + mat[3][3];
    w1 = 1. / w1;

    double xw = mat[0][0]*p.x + mat[0][1]*p.y + mat[0][2]*p.z + mat[0][3];
    double yw = mat[1][0]*p.x + mat[1][1]*p.y + mat[1][2]*p.z + mat[1][3];
    double zw = mat[2][0]*p.x + mat[2][1]*p.y + mat[2][2]*p.z + mat[2][3];

    return Vector(xw * w1, yw * w1, zw * w1);
}

Vector Matrix44::Project(const Vector& p, const double w) const
{
    // XXX Should I put an optimization here for is_identity?

    double w1 = mat[3][0]*p.x + mat[3][1]*p.y + mat[3][2]*p.z + mat[3][3]*w;
    w1 = 1. / w1;

    double xw = mat[0][0]*p.x + mat[0][1]*p.y + mat[0][2]*p.z + mat[0][3]*w;
    double yw = mat[1][0]*p.x + mat[1][1]*p.y + mat[1][2]*p.z + mat[1][3]*w;
    double zw = mat[2][0]*p.x + mat[2][1]*p.y + mat[2][2]*p.z + mat[2][3]*w;

    return Vector(xw * w1, yw * w1, zw * w1);
}

void Matrix44::Project(double &x, double &y, double &z, double &w) const
{
    // XXX Should I put an optimization here for is_identity?

    double x1 = mat[0][0]*x + mat[0][1]*y + mat[0][2]*z + mat[0][3]*w;
    double y1 = mat[1][0]*x + mat[1][1]*y + mat[1][2]*z + mat[1][3]*w;
    double z1 = mat[2][0]*x + mat[2][1]*y + mat[2][2]*z + mat[2][3]*w;
    double w1 = mat[3][0]*x + mat[3][1]*y + mat[3][2]*z + mat[3][3]*w;
    x=x1; y=y1; z=z1; w=w1;
}

Vector Matrix44::ProjectDirection(const Vector& p) const
{
    // XXX Should I put an optimization here for is_identity?

    return Vector(mat[0][0]*p.x+mat[0][1]*p.y+mat[0][2]*p.z,
        mat[1][0]*p.x+mat[1][1]*p.y+mat[1][2]*p.z,
        mat[2][0]*p.x+mat[2][1]*p.y+mat[2][2]*p.z);
}

Vector Matrix44::UnProject(const Vector& p)
{
    // XXX Should I put an optimization here for is_identity?

    if(!inverse_valid)
        compute_inverse();

    double w1 = imat[3][0]*p.x+imat[3][1]*p.y+imat[3][2]*p.z+imat[3][3];
    w1 = 1. / w1;

    double xw = imat[0][0]*p.x+imat[0][1]*p.y+imat[0][2]*p.z+imat[0][3];
    double yw = imat[1][0]*p.x+imat[1][1]*p.y+imat[1][2]*p.z+imat[1][3];
    double zw = imat[2][0]*p.x+imat[2][1]*p.y+imat[2][2]*p.z+imat[2][3];

    return Vector(xw * w1, yw * w1, zw * w1);
}

bool Matrix44::CheckNaN() const
{
    double *m = (double *)mat;
    for(int i=0; i<16; i++)
        ASSERT_RM(!IsNaN(m[i]), "Matrix has a NaN");

    return true;
}
